#ifndef LOCALIZATION_H
#define LOCALIZATION_H

#include <vector>
#include "eigen3/Eigen/Eigen"
#ifdef QT
#   include <QOpenGLFunctions>
#endif

using namespace std;
using namespace Eigen;

class localization
{
private:
    double tread;               // 轮距
    double high;                // 标签高度
    double r;                   // 轮子半径
    double R;                   // 机器人半径

    vector<double> k;           // 优化权重
    vector<Vector3d> uwbs;      // uwb基站坐标
    Vector3d init_center;       // 机器人初始中心坐标
    Vector3d init_tag;          // 标签初始坐标
    Vector3d init_direction;    // 初始方向向量
    Vector3d current_tag;       // 当前标签坐标
    bool first_locate;          // 初次定位标志位
    bool no_init_pose;          // 无初始位姿标志位
    double no_sig_threshold;    // 无信号阈值
    vector< vector<double> > dis_history;

    Transform<double,3,Affine> matrix;        // 位姿矩阵
    Transform<double,3,Affine> matrix_odom;   // 里程矩阵
    vector<double> left;        // 左轮编码器数据
    vector<double> right;       // 右轮编码器数据

    Vector3d locate (vector<double> distance, Vector3d reference, vector<double> weight);    // 根据参考点定位
    Vector3d locate_s (vector<double> distance, Vector3d reference, vector<double> weight);
    double error (vector<double> distance, Vector3d robot);           // 定位残差
    double norm_2 (vector<double> v, vector<double> k);               // 二范数
    double d_model (double x, double y, double theta, double dl, double dr,  Vector3d tag);    // 两轮机器人运动学模型（考虑误差）
    Transform<double,3,Affine> two_wheel_robot_move (double lengthL, double lengthR);          // 两轮机器人运动学模型
    void draw_circle (int pice, double r);                            // opengl绘制圆
    vector<double> calc_std (vector<double> distance, int queue_len);

public:
    localization (vector<Vector3d> uwbs, double high);          // 构造函数，输入uwb基站坐标和机器人高度
    Vector3d locate_tag (vector<double> distance);              // 仅对标签定位，返回标签坐标
    void init_pose (Vector3d center, Vector3d tag);             // 根据机器人中心坐标和标签坐标初始化机器人位姿
    void locate (vector<double> distance);                      // 对机器人定位，更新位姿
    Vector3d get_dir ();                                        // 获取方向向量
    Vector3d get_tag ();                                        // 获取标签坐标
    Vector3d get_center ();                                     // 获取中心点坐标

    void set_speed (double left, double right);                 // 输入机器人两轮运动速度，以及持续的时间片，更新位姿
    void set_speed_angle (double left_speed, double right_speed, double angle, double ratio);
    void set_no_sig_threshold (double threshold);               // 设置无信号阈值
    void set_k (size_t i, double value);                        //设置优化权重
    void draw (double scale);                                   // opengl绘制当前机器人

};

/* 样例伪代码
 *
 * // 初始化
 * uwbs = read_file ("uwb_station_coordinary"); // 读取基站坐标
 * high = read_file ("robot_high");             // 读取机器人高度
 * localization loc (uwbs,high);                // 创建定位对象
 * loc.set_no_sig_threshold (50.0);             // 设置50米为无效信号阈值
 * distance = get_uwb_data ();                  // 读取传感器测距值
 * Vector3d tag = loc.locate_tag (distance);    // 对标签定位
 * control_robot_turn_180 ();                   // 控制机器人自转180度
 * Vector3d tag2 = loc.locate_tag (distance);   // 对标签定位
 * Vector3d center = (tag_tag2)/2;              // 计算机器人中心坐标
 * control_robot_turn_180 ();                   // 控制机器人自转180度
 * loc.init_pose (center,tag);                  // 初始化机器人位姿
 *
 * Vector3d robot_center;
 * Vector3d robot_dir;
 *
 * // 主线程
 * while (1) {
 *     motion_control (robot_center,robot_dir);            // 根据业务逻辑进行运动控制
 * }
 *
 * // IO线程
 * while (1) {
 *     data = get_encoder_data ();                         // 读取传感器值
 *     if (data.type()=="encoder")                         // 如果是编码器数据
 *         loc.set_speed (data.left, data.right, time);        // 设置速度，更新位姿
 *     if (data.type()=="uwb")                             // 如果是uwb测量信息
 *         loc.locate (data.distance);                         // 定位，更新位姿
 *     robot_center = loc.get_center;                      // 读出机器人中心坐标到全局变量
 *     robot_dir = loc.get_dir;                            // 读出机器人方向向量到全局变量
 * }
 *
 */

#endif // LOCALIZATION_H
